/*
 Copyright (c) 2013, Francisco Suárez-Ruiz
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright
 notice, this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright
 notice, this list of conditions and the following disclaimer in the
 documentation and/or other materials provided with the distribution.
 * Neither the name of the <organization> nor the
 names of its contributors may be used to endorse or promote products
 derived from this software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY Francisco Suárez <email> ''AS IS'' AND ANY
 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL Francisco Suárez <email> BE LIABLE FOR ANY
 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/

#include "ros/ros.h"
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "std_msgs/Float64.h"
#include "UDPwriter.h"
#include "UDPreader.h"

#define JOINTS_PER_ROBOT 	6
#define ROBOTS				2

enum {
	STOP_STATE, START_STATE, HALT_STATE, RESUME_STATE
};

//global variables for sockets
UDPwriter *udp_writer;
UDPreader *udp_reader;

/** Error call for the non-ROS system functions */
void error(const char *msg) {
	ROS_ERROR("<udp_interface> gave an error : [%s]", msg);
	exit(1);
}

/*Callback for when a message is received to be sent */
void joints_subscriber(const sensor_msgs::JointState::ConstPtr& msg) {
	int i, num_joints = msg->name.size();
//	ROS_DEBUG("Joints read: [%d]", num_joints);
	float position[num_joints - 1], velocity[num_joints-1], effort[num_joints-1];
	for (i = 0; i < num_joints; i++) {
		position[i] = -(float) msg->position.at(i);
		if (i == 2)
			position[i] = M_PI - position[i] - position[i-1];
		velocity[i] = (float) msg->velocity.at(i);
		effort[i] = (float) msg->effort.at(i);
	}
	if (udp_writer->send(position) < 0)
		error("ERROR sending <positions> trough UDP");
}
int main(int argc, char **argv) {
	//Initialize node
	ros::init(argc, argv, "udp_interface");

	//check usage is correct
	if (argc != 4) {
		ROS_INFO("usage: udp_interface REMOTE_ADDRESS WRITE_PORT READ_PORT");
		return 1;
	}
	//	Initialize socket elements
	const char *ip_address = argv[1];
	int write_port = atoi(argv[2]), read_port = atoi(argv[3]);
	udp_writer = new UDPwriter(ip_address, write_port);
	if (udp_writer->create() < 0)
		error("Failed to create the <udp_writer>");
	udp_reader = new UDPreader(read_port);
	if (udp_reader->create() < 0)
		error("Failed to create the <udp_reader>");
	if (udp_reader->bind_socket() < 0)
		error("Failed to bind <udp_reader> socket");

	// Set up the subscriber topic for messages (using UDPROS)
	ros::NodeHandle n;
	ros::Subscriber joint_states_sub = n.subscribe("joint_states", 1,
			joints_subscriber, ros::TransportHints().udp());
	ros::Rate rate(1000);

	// Set up JointPositionController publishers
	std_msgs::Float64 cmd_msg;
	int i, j, number_of_joints = JOINTS_PER_ROBOT * ROBOTS;
	std::vector<ros::Publisher> pub_joint_controllers;
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/r_SA_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/r_SE_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/r_EL_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/r_WP_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/r_WY_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/r_WR_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/l_SA_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/l_SE_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/l_EL_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/l_WP_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/l_WY_controller/command", 1));
	pub_joint_controllers.push_back(
			n.advertise<std_msgs::Float64>("/l_WR_controller/command", 1));

	// Start ROS listening
//	ROS_INFO("Waiting for topic <joint_states> to send to PXI");
//	ROS_INFO("Waiting for UDP data from PXI");
	ROS_INFO("Listening on Port: [ %d ]", read_port);
	ROS_INFO("Writing on Port: [ %d ]", write_port);
	while (ros::ok()) {
		rate.sleep();
		const std::vector<float> &udp_recv_vector = udp_reader->receive();
		if (((int) udp_recv_vector.size()) == number_of_joints + ROBOTS) {
			int pub_index = 0;
			for (i = 0; i < ROBOTS; i++) {
				int pivot = i*(JOINTS_PER_ROBOT + 1);
				if ((int) udp_recv_vector.at(pivot) == RESUME_STATE) {
					int robot_index = i+1;
					for (j = pivot+1; j < robot_index*JOINTS_PER_ROBOT; j++) {
						if ((j == 3) || (j == 9))
							cmd_msg.data = - (M_PI  - (double) udp_recv_vector.at(j - 1) - (double) udp_recv_vector.at(j));
						else
							cmd_msg.data = -(double) udp_recv_vector.at(j);
						pub_joint_controllers.at(pub_index).publish(cmd_msg);
						pub_index ++;
					}
				}
			}
		}
		ros::spinOnce();
	}
	udp_writer->close_socket();
	udp_reader->close_socket();
	return 0;
}
